function [output] = my_speed2angle(input)
% 速度按列向量传入,输出为列向量,采用远程火箭常用的坐标系转换方式 2-3-1
n = size(input,2);
output = zeros(3,n);

for i=1:1:n
    q = input(:,i);
    q = q/norm(q);
    q_0 = q(1);q_1 = q(2);q_2 = q(3);q_3 = q(4);

    rotage_matrix = [-2 * q_2 * q_2 - 2 * q_3 * q_3 + 1,2 * q_0 * q_3 + 2 * q_1 * q_2,2 * q_1 * q_3 - 2 * q_0 * q_2;
                    2 * q_1 * q_2 - 2 * q_0 * q_3, -2 * q_1 * q_1 - 2* q_3 * q_3 + 1, 2 * q_0 * q_1 + 2 * q_2 * q_3;
                    2 * q_0 * q_2 + 2 * q_1 * q_3, 2 * q_2 * q_3 - 2 * q_0 * q_1, -2 * q_1 * q_1 - 2 * q_2 * q_2 + 1];

    C_II_origin = [0, 1, 0;
              0, 0, 1;
              1, 0, 0];
    rotage_matrix = rotage_matrix * C_II_origin;

    phic = rad2deg(atan2(rotage_matrix(1,2),rotage_matrix(1,1)));
    psic = rad2deg(asin(-rotage_matrix(1,3)));
    gammac = rad2deg(atan2(rotage_matrix(2,3),rotage_matrix(3,3)));
    output(:,i) = [phic;psic;gammac];
end
end